﻿/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_BASICSENSOR_H_
#define __MMM_BASICSENSOR_H_

#include "../MMMCore.h"
#include "../MMMImportExport.h"
#include "../Model/Model.h"
#include "Sensor.h"
#include "SensorMeasurement.h"
#include "../RapidXML/RapidXMLReader.h"
#include "../RapidXML/RapidXMLWriter.h"
#include "../MathTools.h"
#include "../XMLTools.h"

#include <string>
#include <vector>
#include <map>
#include <cmath>

namespace MMM
{

// M : SensorMeasurement, Cloneable<M>
template <typename M, typename std::enable_if<std::is_base_of<SensorMeasurement, M>::value>::type* = nullptr, typename std::enable_if<std::is_base_of<SMCloneable<M>, M>::value>::type* = nullptr>
/*! \brief The templated interface with already implemented methods for implementing new sensor classes. */
class MMM_IMPORT_EXPORT BasicSensor : public Sensor
{

public:

    virtual bool checkModel(ModelPtr model) = 0;

    virtual boost::shared_ptr<BasicSensor<M> > cloneConfiguration() = 0;

    virtual bool equalsConfiguration(SensorPtr other) = 0;

    SensorPtr clone() {
        return cloneDerived();
    }

    boost::shared_ptr<BasicSensor<M> > cloneDerived() {
        boost::shared_ptr<BasicSensor<M> > clonedSensor = cloneConfiguration();
        for (auto const &measurement : measurements) {
            clonedSensor->measurements[measurement.first] = measurement.second->clone(measurement.first);
        }
        return clonedSensor;
    }

    void appendSensorXML(RapidXMLWriterNodePtr node)
    {
        assert(node);

        if(!hasSensorConfigurationContent()) {
          return;
        }

        RapidXMLWriterNodePtr sensorNode = node->append_node(XML::NODE_SENSOR);
        sensorNode->append_attribute(XML::ATTRIBUTE_TYPE, getType());
        sensorNode->append_attribute(XML::ATTRIBUTE_VERSION, getVersion());
        if (!name.empty()) {
          sensorNode->append_attribute(XML::ATTRIBUTE_NAME, name);
        }
        if (!description.empty()) {
          sensorNode->append_attribute(XML::ATTRIBUTE_DESCRIPTION, description);
        }
        appendConfigurationXML(sensorNode);
        RapidXMLWriterNodePtr dataNode = sensorNode->append_node(XML::NODE_SENSORDATA);
        for (auto const &measurement : measurements) {
            measurement.second->appendDataXML(dataNode);
        }
    }

    bool hasMeasurement(float timestep) {
        return measurements.find(timestep) !=  measurements.end();
    }

    SensorPtr getSegmentSensor(float startTimestep, float endTimestep, bool changeTimestep = false)
    {
        assert(startTimestep < endTimestep);

        float e = 0.000001f;
        boost::shared_ptr<BasicSensor<M> > segmentSensor = cloneConfiguration();
        std::map<float, boost::shared_ptr<M> > segmentMeasurements;
        auto it = measurements.upper_bound(startTimestep - e);
        float firstTimestep = it->first;
        while(it != measurements.end() && it->first < endTimestep + e) {
            float timestep = changeTimestep ? Math::roundf(it->first - firstTimestep) : it->first;
            segmentMeasurements[timestep] = it->second->clone(timestep);
            ++it;
        }
        segmentSensor->measurements = segmentMeasurements;
        return segmentSensor;
    }

    SensorMeasurementPtr getMeasurement(float timestep) {
        return getDerivedMeasurement(timestep);
    }

    virtual boost::shared_ptr<M> getDerivedMeasurement(float timestep)
    {
        if (hasMeasurement(timestep)) return measurements[timestep];
        else return nullptr;
    }

    SensorMeasurementPtr getMeasurement(float timestep, float delta) {
        return getDerivedMeasurement(timestep, delta);
    }

    virtual boost::shared_ptr<M> getDerivedMeasurement(float timestep, float delta) {
        if (hasMeasurement(timestep)) return measurements[timestep];
        else {
            auto it = measurements.lower_bound(timestep);
            if (it != this->measurements.end()) {
                if (it != this->measurements.begin()) {
                    auto prev = std::prev(it);
                    float delta1 = std::abs(it->first - timestep);
                    float delta2 = std::abs(prev->first - timestep);
                    boost::shared_ptr<M> measurement = (delta1 < delta2) ? it->second : prev->second;
                    if (std::abs(measurement->getTimestep() - timestep) < delta + 0.000001) return measurement;

                }
            }
        }
        return nullptr;
    }

    virtual bool addSensorMeasurement(boost::shared_ptr<M> measurement)
    {
        measurements.insert(std::pair<float, boost::shared_ptr<M>>(measurement->getTimestep(), measurement));
        return true;
    }

    std::vector<float> getTimesteps() {
        std::vector<float> timesteps;
        for (const auto &measurement : measurements) timesteps.push_back(measurement.first);
        return timesteps;
    }

    std::map<float, SensorMeasurementPtr> getMeasurements() {
        std::map<float, SensorMeasurementPtr> m;
        for (const auto &measurement : measurements) m.insert(std::pair<float, SensorMeasurementPtr>(measurement.first, measurement.second));
        return m;
    }

    float getMinTimestep() {
        if (measurements.size() == 0) return -1;
        return measurements.begin()->first;
    }

    float getMaxTimestep() {
        if (measurements.size() == 0) return -1;
        return measurements.rbegin()->first;
    }

    virtual void shiftMeasurements(float delta) {
        std::map<float, boost::shared_ptr<M> > shiftedMeasurements;
        for (const auto &m : measurements) {
            float newTimestep = m.second->getTimestep() + delta;
            shiftedMeasurements[newTimestep] = m.second->clone(newTimestep);
        }
        measurements = shiftedMeasurements;
    }

    void extend(float minTimestep, float maxTimestep) {
        boost::shared_ptr<M> minMeasurement = measurements.begin()->second;
        boost::shared_ptr<M> maxMeasurement = measurements.rbegin()->second;
        if (minMeasurement->getTimestep() > minTimestep) {
            boost::shared_ptr<M> newMinMeasurement = minMeasurement->clone(minTimestep);
            newMinMeasurement->setType(SensorMeasurementType::EXTENDED);
            measurements[minTimestep] = newMinMeasurement;
        }
        if (maxMeasurement->getTimestep() < maxTimestep) {
            boost::shared_ptr<M> newMaxMeasurement = maxMeasurement->clone(maxTimestep);
            newMaxMeasurement->setType(SensorMeasurementType::EXTENDED);
            measurements[maxTimestep] = newMaxMeasurement;
        }
    }

protected:
    virtual void loadMeasurementXML(RapidXMLReaderNodePtr node, float timestep, SensorMeasurementType type) = 0;
    virtual void loadConfigurationXML(RapidXMLReaderNodePtr node) = 0;
    virtual void appendConfigurationXML(RapidXMLWriterNodePtr node) = 0;
    virtual bool hasSensorConfigurationContent() const = 0;

    BasicSensor(const std::string &description = std::string(), const std::map<float, boost::shared_ptr<M> > &measurements = std::map<float, boost::shared_ptr<M> >()) :
        Sensor(description),
        measurements(measurements)
    {
    }

    void loadMeasurementNodeXML(RapidXMLReaderNodePtr node) {
        float timestep = XML::readFloat(node->attribute_value(XML::ATTRIBUTE_TIMESTEP).c_str());
        SensorMeasurementType type = SensorMeasurementType::MEASURED;
        if (node->has_attribute(XML::ATTRIBUTE_TYPE)) {
            std::string value = node->attribute_value(XML::ATTRIBUTE_TYPE);
            type = SensorMeasurementType::toSensorMeasurementType(value);
            if (type == SensorMeasurementType::UNKNOWN) {
                MMM_INFO << "Added sensor measurement with unknown type '" << value << "' in sensor " << getUniqueName() << " at timestep " << timestep << std::endl;
            }
        }
        loadMeasurementXML(node, timestep, type);
    }

    void loadSensor(RapidXMLReaderNodePtr node)
    {
        assert(node);
        assert(node->name() == XML::NODE_SENSOR);
        assert(node->attribute_value(XML::ATTRIBUTE_TYPE) == getType());

        this->name = node->attribute_value_or_default(XML::ATTRIBUTE_NAME, std::string());
        this->description = node->attribute_value_or_default(XML::ATTRIBUTE_DESCRIPTION, std::string());

        loadConfigurationXML(node->first_node(XML::NODE_SENSORCONFIGURATION));
        for (auto measurementNode : node->first_node(XML::NODE_SENSORDATA)->nodes(XML::NODE_MEASUREMENT)) loadMeasurementNodeXML(measurementNode);
    }

    SensorPtr joinSensor(SensorPtr sensor) {
        if(!equalsConfiguration(sensor)) return nullptr; // Prüfe Sensor auf Gleichheit
        else {
            boost::shared_ptr<BasicSensor<M> > clonedSensor = cloneDerived();
            boost::shared_ptr<BasicSensor<M> > cast_sensor = boost::dynamic_pointer_cast<BasicSensor<M> >(sensor); // TODO: Kann der Cast umgangen werden?
            for (const auto &measurement : cast_sensor->measurements) {
                if (measurements.find(measurement.first) == measurements.end()) {
                    clonedSensor->measurements[measurement.first] = measurement.second->clone(measurement.first);
                }
                else if (!clonedSensor->measurements[measurement.first]->equals(measurement.second)) return nullptr; // Prüfe Measurements auf Gleichheit
            }
            return clonedSensor;
        }
    }

    std::map<float, boost::shared_ptr<M> > measurements;
};
}

#endif // __MMM_BASICSENSOR_H_
